__author__ = 'kilo'

import numpy as np

# ==================================== Aufgabe 1 ===============================================================
print "========================================================================="
print  "Anfang Aufgabe 1"
print "Kameramatrix: "
kameramatrix = np.array([[490,-390,-1500,1300],[-590,1400,-600,1300],[-0.5*np.sqrt(2),-0.3*np.sqrt(2),-0.4*np.sqrt(2),5]])
print kameramatrix

x1 =kameramatrix[:,[1,2,3]]
# determinante berechnen von x1
determinante_x1=np.linalg.det(x1)

x2 = kameramatrix[:,[0,2,3]]
# determinante berechnen von x2
determinante_x2=np.linalg.det(x2)

x3 = kameramatrix[:,[0,1,3]]
# determinante berechnen von x3
determinante_x3=np.linalg.det(x3)

x4 = kameramatrix[:,[0,1,2]]
# determinante berechnen von x4
determinante_x4 = np.linalg.det(x4)

# punkte berechnung
pkt_1 = determinante_x1/determinante_x4
pkt_2= determinante_x2/determinante_x4
pkt_3 = determinante_x3/determinante_x4
pkt_4 = determinante_x4/determinante_x4

kamerazentrum = np.array((pkt_1,pkt_2,pkt_3))
print "Kamerazentrum:"
print kamerazentrum

# manuelle Cholesky Zerlegung
M = np.array([[490,-390,-1500],[-590,1400,-600],[-0.5*np.sqrt(2),-0.3*np.sqrt(2),-0.4*np.sqrt(2)]])
MT = np.array([[490,-590,-0.5*np.sqrt(2)],[-390,1400,-0.3*np.sqrt(2)],[-1500,-600,-0.4*np.sqrt(2)]])
erg = np.dot(M,MT)
print 'ERG ', erg

# matrix aufstellen
a = erg[0,0]
b = erg[0,1]
kamerazentrum = erg[2,0]
d = erg[1,1]
e = erg[2,1]

# zuweisung und berechnungen
xv = e
xu = kamerazentrum
kv = np.sqrt(d-(e*e))
s = (b-kamerazentrum*e)/kv
ku= np.sqrt(a-(((b-kamerazentrum*e)*(b-kamerazentrum*e))/(d-e*e))-kamerazentrum*kamerazentrum)
print 'ku ', ku
# kalibriermatrix erstellen
k = np.array(((ku,s,xu),(0,kv,xv),(0,0,1)))
print"\nKalibriermatrix:"
print k

# r berechnung
print 'M ', M
print 'k', k
r = np.linalg.inv(k)*M
print "Die Orientierung"
print r
print  "Ende Aufgabe 1"

# ================================= Aufgabe 2 =============================================================
print "========================================================================="
print " Anfang Aufgabe 2 "
# Angaben
f = 6
pixel = 0.005
pixel = 1/pixel
sensor = (640, 480)
h = (310,250)
C = np.array((100, 200, 300))

# Berechnungen
R = np.array(((1, 0, 0), (0, 1, 0), (0, 0, 1)))
I = np.array(((1, 0, 0, -C[0]), (0, 1, 0, -C[1]), (0, 0, 1, -C[2])))
k1 = np.array(((pixel, 0, 0), (0, pixel, 0), (0, 0, 1)))
k2 = np.array(((f, 0, h[0]), (0, f, h[1]), (0, 0, 1)))
k = np.dot(k1, k2)

kameramatrix = np.dot(np.dot(k,R), I)
print "Kameramatrix P:"
print kameramatrix
print  "Ende Aufgabe 2"
print "========================================================================="